/*
  //============\\
 || PEDAL NODE ||
 \\============//
 
 Analog Signals:
 ---------------
 - Accelerator_1
 - Accelerator_2
 - Break_1
 - Break_2
 
 Digital Signals:
 ----------------
 - Accelerator Up (IVS)
 - Break Over Travel (BOT)
 
 Functionality Summary:
 ----------------------
 Read values and broadcast them over CAN bus.
 
 Communication Protocol:
 -----------------------
 - CAN Byte Order
 0 ) Accel
 1 ) Brake
 2 ) Accelerator 1
 3 ) Accelerator 2
 4 ) Brake 1
 5 ) Brake 2
 6 ) { 0, 0, 0, 0, 0, 0, IVS, BOT }
 7 ) 
 
 Arduino Uno Pin Assignemnts:
 ----------------------------
  0 ) RX - Debug
  1 ) TX - Debug
  2 ) 
  3 ) 
  4 ) 
  5 ) 
  6 ) 
  7 ) 
  8 ) 
  9 ) 
 10 ) 
 11 ) BOT
 12 ) IVS
 13 ) Flash Alive LED
 A0 ) Accelerator 1
 A1 ) Accelerator 2
 A2 ) Brake 1
 A3 ) Brake 2
 A4 ) 
 A5 ) 
 */

//#include <SPI.h>
//#include "CAN_lib.h"
#include <GtCAN_lib.h>

// Analog
#define ACCEL_1  A0
#define ACCEL_2  A1
#define BRAKE_1  A2
#define BRAKE_2  A3
// Digital
#define LED  13
#define IVS  12
#define BOT  11


GtCAN_lib can(10,0);
unsigned int resolution = 100;
unsigned int loop_delay;
unsigned int brake_threshold = 50;
char alive = 0;
int CAN_id = PedalNodeID;
int arr[7];

void setup() {
  Serial.begin(9600);
  pinMode(ACCEL_1, INPUT);
  pinMode(ACCEL_2, INPUT);
  pinMode(BRAKE_1, INPUT);
  pinMode(BRAKE_2, INPUT);
  pinMode(IVS, INPUT);
  pinMode(BOT, INPUT);
  pinMode(LED, OUTPUT);
  loop_delay = 1000 / resolution;
}

void loop() {
  // Flash Alive
  digitalWrite(LED, alive);
  alive = !alive;
  
  // read values
  arr[2] = analogRead(ACCEL_1);
  arr[3] = analogRead(ACCEL_2);
  arr[4] = analogRead(BRAKE_1);
  arr[5] = analogRead(BRAKE_2);
  arr[6] = (digitalRead(IVS) < 1) | digitalRead(BOT);

  // Handle Break
  arr[1] = (arr[4]+arr[5])>>1;
  if ((arr[6] & 1) > 0)
    arr[1] = 0xFF;

  // Handle Accelerator
  arr[0] = (arr[2]+arr[3])>>1;
  if (arr[1] >= brake_threshold || (arr[6] & 2) > 0)
    arr[0] = 0;

  // Send Data
  tCAN msg;
  msg.id = CAN_id;
  msg.length = 7;
  for(int i=0;i<msg.length;i++)
    msg.buff[i] = arr[i];
  if (can.sendMessage(&msg)) {
    // message sent
  } 
  else {
    // error sending message
  }
  delay(loop_delay);
}

